/*
 * Copyright (c) 2006-2021, Halo team
 *
 * SPDX-License-Identifier: GPLv3
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-09-11     Eywen        the first version
 */

#include "motion_step.h"
#include "common.h"

static float motion_step_speed_to_freq(float speed, motion_step_type_t step_type);
static float motion_step_distance_to_pulse(motion_step_t *step, float distance);
static float motion_step_pulse_to_distance(uint32_t pulse, motion_step_type_t step_type);
static bool motion_step_add_task(motion_step_t *device, motion_step_task_t *task);
static void motion_step_forward_level_set(motion_step_t *step, bool level);
static motion_step_err_t motion_step_error_get(motion_step_t *device);
static void motion_step_axis_range_set(motion_step_t *step, float axis_length);
static void motion_step_axis_zero_offset_set(motion_step_t *step, float offset);
static bool motion_step_task_start(motion_step_t *step);
static bool motion_step_task_full(motion_step_t *step);
static bool motion_step_is_motion(motion_step_t *step);

/*
 *  计算电机的转动速度
 * freq：控制脉冲的频率，
 * z   ：转子的齿数
 * n   ：运行的拍数
 *  */
float step_rotational_speed_cal(uint32_t freq, uint32_t z, uint32_t n)
{
    return (60*freq)/(z*n);     /* unit: r/min */
}

bool motion_step_init(motion_step_t *device, motion_step_type_t type,
                      smart_step_id_t id ,int pin_pwm, int pin_en, int pin_dir,
                      uint32_t timer_freq)
{
    ASSERT_NULL(device);
    smart_step_t *smart_step;
    bool ret;
    device->type = type;

    smart_step = device->step_drv;
    ret = smart_step->ops->init(smart_step, pin_pwm, pin_en, pin_dir, timer_freq, id);
    if(ret == false){
        goto  err;
    }

    return true;
err:
    return false;
}

static const float s_rate_dis_pulse[step_type_max] = {
    [step_x] = 50,
    [step_y] = 50,
    [step_z] = 100,
    [step_xy] = 0,
    [step_x_left] = 0,
    [step_x_right] = 0
};

/* 距离转换成脉冲个数 */
static inline float motion_step_distance_to_pulse(motion_step_t *step, float distance)
{
    UNUSED(step);
    return distance*s_rate_dis_pulse[step->type];
}

/* 脉冲数转换成距离长度 */
inline float motion_step_pulse_to_distance(uint32_t pulse, motion_step_type_t step_type)
{
    UNUSED(step_type);
    return (float)pulse;
}


/* 将步进电机移动的速度转换成驱动的频率 */
float motion_step_speed_to_freq(float speed, motion_step_type_t step_type)
{
    UNUSED(step_type);
    return speed;
}

/* 获取当前电机的错误状态 */
inline motion_step_err_t motion_step_error_get(motion_step_t *device)
{
    return device->err_status;
}

/* 将需要移动目标距离和速度送入到地层的驱动任务队列*/
bool  motion_step_add_task(motion_step_t *device, motion_step_task_t *task)
{
    ASSERT_NULL(device);
    ASSERT_NULL(task);
    smart_step_t *step;
    smart_step_task_t pwm_task;
    step = device->step_drv;
    pwm_task.freq = motion_step_speed_to_freq(task->speed, device->type);
    pwm_task.pulse = (uint32_t)motion_step_distance_to_pulse(step, task->distace);
    return step->ops->add_task(step, &pwm_task);
}

/* 设置电机能转动的最大长度 */
inline void motion_step_axis_range_set(motion_step_t *step, float axis_length)
{
    ASSERT_NULL(step);
    step->axis_length = axis_length;
}

/* 设置home地址的偏移值 */
inline void motion_step_axis_zero_offset_set(motion_step_t *step, float offset)
{
    ASSERT_NULL(step);

    step->home_offset = offset;
}

/* 获取当前位置(相对于home地址,非绝对的0位置) */
 float motion_step_axis_coordinate(motion_step_t *step)
 {
     ASSERT_NULL(step);
     uint32_t pulse;
     float abs_loc;
     smart_step_t *smart_step;
     smart_step = step->step_drv;
     pulse = smart_step->ops->pulse(smart_step);
     abs_loc = motion_step_pulse_to_distance(pulse, step->type);
     return (abs_loc - step->home_offset);
 }

 /* 设置电机争相转动方向的标准电平 */
inline void motion_step_forward_level_set(motion_step_t *step, bool level)
{
    ASSERT_NULL(step);
    smart_step_t *smart_step;
    smart_step = step->step_drv;
    smart_step->ops->forward_level_set(smart_step, level);
}

static bool motion_step_task_start(motion_step_t *step)
{
    ASSERT_NULL(step);
    ASSERT_NULL(step->step_drv);
    ASSERT_NULL(step->step_drv->ops);

    step->step_drv->ops->start(step->step_drv);
    return true;
}

static bool motion_step_task_full(motion_step_t *step)
{
    ASSERT_NULL(step);
    return step->step_drv->ops->task_is_full(step->step_drv);
}


static bool motion_step_task_empty(motion_step_t *step)
{
    ASSERT_NULL(step);
    return step->step_drv->ops->task_is_empty(step->step_drv);

}

static bool motion_step_is_motion(motion_step_t *step)
{
    ASSERT_NULL(step);
    return step->step_drv->ops->is_motion(step->step_drv);
}


/* 运动电机操作接口 */
const static motion_step_ops_t s_motion_step_ops = {
    .init = motion_step_init,
    .add_task = motion_step_add_task,
    .error = motion_step_error_get,
    .start_task = motion_step_task_start,
    .axis_range_set = motion_step_axis_range_set,
    .axis_zero_offset_set = motion_step_axis_zero_offset_set,
    .coordinate = motion_step_axis_coordinate,
    .forward_level_set = motion_step_forward_level_set,
    .task_full = motion_step_task_full,
    .task_empty = motion_step_task_empty,
    .is_motion = motion_step_is_motion,
    .distance_to_pulse = motion_step_distance_to_pulse,
};



/* 注册运动电机 */
bool motion_step_register(motion_step_t *device, motion_step_ops_t *ops)
{
    debug_info("motion step register");

    ASSERT_NULL(device);
    ASSERT_NULL(ops);

    MEM_MEMSET(device, 0, sizeof(motion_step_t));

    device->ops = ops;

    /* 申请步进电机驱动控制内存  */
    device->step_drv = MEM_MALLOC(sizeof (smart_step_t));
    if(device->step_drv == NULL){
        goto err;
    }

    smart_step_t *smart_step;
    smart_step = device->step_drv;

    /* 注册驱动 */
    if(false == smart_step_register(smart_step)){
        goto err;
    }

    debug_info("motion step register success");
    return true;
err:
    /* 初始化出错, 释放申请的内存*/
    debug_error("motion step register fail");
    if(device->step_drv){
        MEM_FREE(device->step_drv);
        device->step_drv = NULL;
    }
    return false;
}

motion_step_ops_t * motion_step_ops_get(motion_step_type_t type)
{
    debug_info("motion step ops get");
    UNUSED(type);
    return (motion_step_ops_t *)&s_motion_step_ops;
}
